# Sap-Comm.tcl - Simulation for ArduPilot Comms/packet functionality
# 
# Copyright (C) 2010  Paul Hopkins  email:- hopslink@gmail.com
#
#    This program is free software: you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation, either version 3 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program.  If not, see <http://www.gnu.org/licenses/>.
#


package require udp 1.0.8

namespace eval ::sap {
	variable S	;# namespace variable
	
	set S(comm.fgHost) 	127.0.0.1	;# hostname/IP for flightgear host
	set S(comm.fgPort) 	5010		;# port for data from flightgear
	set S(comm.fgChan) 	{}		;# channel for data to flightgear
	
	set S(comm.fgOutSocket) {}
	set S(comm.fgOutChan) 	{}		;# channel for data to flightgear
	set S(comm.fgOutFlag)	0		;# flag for channel configured and connected
	set S(comm.fgServicePort)	5020	;# port for data to flightgear
	
	set S(comm.SerialPort) 	{}		;# serial port 
	set S(comm.SerialChan)	{}		;# handle for open serial channel
	set S(comm.SerialSelector) {}		;# handle for serial port selector combobox
	set S(comm.SerialBaudRate) 38400	;# 
	
	set S(comm.fgInputCount) 0		;# fg input packet counter
	set S(comm.fgGPSPacketCount)	0	;# gps packet counter used for sync indication

	# Ground Station stubs - currently unused
#	set S(comm.gsChan)	{}		;# channel for comms with groundstation
#	set S(comm.gsHost)	127.0.0.1	;# groundstation host address
#	set S(comm.gsPort)	5070		;# groundstation socket no. 
	
}


###################################################################################################################
# Data reformatting ROUTINES 


# imulate - reformat flightgear output data to match ArduImu output
# arguments:
# FGData - An output packet from flightgear
# 
# output - ArduImu packets sent to ArduPilot via serial link
#
proc ::sap::imulate {FGData} {
	variable S
	
	# check we have some data
	if {[string length $FGData] < 1 } {
		return
	}
	
	# incoming data is read as a single string
	# - split into a Tcl list (array of strings)
	set valArray [split $FGData ","]
	
	# convert heading data from 0 - 360 to -180 - +180 (degrees x100)
	set correctedHeading [lindex $valArray 2]	
	if {$correctedHeading > 18000} {
		set correctedHeading [expr {$correctedHeading - 36000}]
		# replace value 
		lset valArray 2 $correctedHeading
	}
	 	
	# Add extra data - heading again and iTOW
	lappend valArray $correctedHeading
	lappend valArray [clock milliseconds]	;# an approximation for iTOW?
	
	# flightgear data - converted to integers
	set fgIntData [list {}]	
	foreach x $valArray {
		lappend fgIntData [expr { int( $x ) } ]
	}
	
#	#DEBUG - print all variables in a list
#	set i 0
#	foreach x $fgIntData {
#		puts "fgIntData $i = $x"
#		incr i
#	}
#	#/DEBUG
	
	# add header and convert to binary
	set imuMesgOut [binary format a4ccs3 "DIYd" 6 2 [lrange $fgIntData 1 3] ] 
		
	# calculate checksum 
	set imuMesgOut [::sap::imuChecksum $imuMesgOut]

	#send to ArduPilot
	puts -nonewline $::sap::S(comm.SerialChan) $imuMesgOut
	flush $::sap::S(comm.SerialChan)	
	#puts "sent $imuMesgOut"	;#DEBUG - print message
	
	# log data values and timestamp for IMU packet
	set logList [concat [list "IMU"] [lrange $fgIntData 1 3] [lindex $fgIntData end]  ]
	set logString [join $logList ","]		;# log as csv string
	lappend S(log.Data) $logString
		
	# gps data output 5Hz (every 10th packet)	
	if { $S(comm.fgInputCount) == 9 } {		
		
		# add header and convert to binary
		# use the imu health byte as a packet counter/tracer
		set gpsMesgOut [binary format a4cci2s3ic \
		 	"DIYd" 19 3 [lrange $fgIntData 4 5] [lrange $fgIntData 6 8] \
		 	[lindex $fgIntData 9] $S(comm.fgGPSPacketCount) ]
        	#TODO get value for imuhealth from ArduImu code
		
		#tick it - calculate checksum and add to packet
		set gpsMesgOut [::sap::imuChecksum $gpsMesgOut]

		#send to ArduPilot
        	puts -nonewline $::sap::S(comm.SerialChan) $gpsMesgOut
        	flush $::sap::S(comm.SerialChan)
		#puts "sent $gpsMesgOut"	;#DEBUG - print message
				
		# log data values and timestamp for SPS packet
		set logList [concat [list "GPS"] [lrange $fgIntData 4 end] ]
		# use the imu health byte as a packet tracer
		lappend logList $S(comm.fgGPSPacketCount)
		
		set logString [join $logList ","]	;# log as csv string
		lappend S(log.Data) $logString
		
		incr S(comm.fgGPSPacketCount)
		
		if { $S(comm.fgGPSPacketCount) > 99 } {
			set S(comm.fgGPSPacketCount) 0
		}
		
#		#DEBUG 
#		update
				
		set S(comm.fgInputCount) 0	;# reset counter
	} else {
		incr S(comm.fgInputCount)	;# count packet
	}	
}


# imuChecksum
# calculates ArduIMU checksum for a string of binary data
#
# arguments:
#  message - a binary data string
#
# returns:
#  message with checksum bytes appended
#
proc ::sap::imuChecksum {message} {
	# split data into a list of bytes
	binary scan $message cu* byteList
	
	# do checksum
	set chk_1 0
	set chk_2 0
	foreach x [lrange $byteList 4 end] {
		set chk_1 [ expr { int ( $chk_1 + $x ) } ]
		set chk_2 [ expr { int ( $chk_2 + $chk_1 ) } ]
	}
	
	# add checksum to string
	set imuCheckSum [list $chk_1 $chk_2]
	set checkString [binary format c* $imuCheckSum]	;# convert to 2 binary bytes
	append message $checkString
	
	return $message
}

###################################################################################################################
#SERIAL ROUTINES

# apSerialRead - handles read events for serial data 
# passes telemetry data to GUI for display
# and control surface data to flightgear via udp
#
# arguments:
#   serialChan - handle to the serial comms channel
# 
proc ::sap::apSerialRead {serialChan} {
	variable S

	if { [catch {gets $serialChan serialDataIn} err] } { 		;# read line of data from AP
        		set errType [fconfigure $serialChan -lasterror]		;# IF error report extra data to user 
        		puts "Tcl reports serial error type - $errType "	;
        		#TODO handle for OS's other than MSWin
    	} else {							;# ELSE no error - use the data
		
		# check we have some data
        	if {[string length $serialDataIn] < 1 } {
        		return
        	}   
		
		# timestamp and send to ground station for logging and display
		set timeString [expr { int( [clock milliseconds] ) }]
		    
		lappend ::sap::S(log.Data) "$serialDataIn,$timeString"		    
    		#puts "IN - $serialDataIn"		;# DEBUG display on stdout
		    
		if {[string range $serialDataIn 0 2] == "!!!"} {				
                    	# display ArduPilot data
                    	::sap::win_displayInData $serialDataIn			
                } 
		
		#Check we have FG connection - else just act as a basic GroundStation
		if {$::sap::S(comm.fgOutFlag)} {
			
        		#check for ArduPilot attitude packet
			if {[string range $serialDataIn 0 2] == "+++"} {
				
				# if correct packet parse out servo position data
				set fgDataList [split $serialDataIn ",:"]
				
				# find the start of AP servo pulsewidth data
				set servoDataIndex [lsearch -exact $fgDataList "SIM"]
				# quit if we can't find it
				if {$servoDataIndex < 0} {
					return
				}
								
				# increment pointer to point at 
				incr servoDataIndex
				set servoDataEnd [expr {$servoDataIndex + 2}]
				
				# try to get the servo pulsewidth data from the packet
				if { [catch {set servoPWData [lrange $fgDataList $servoDataIndex $servoDataEnd ]}]} {
                                    return
                                }
				
				# grab AP servo pulsewidth data
				set fgOutData [list]
				# convert AP servo Pulsewidths (microseconds) to FG normalised controls values
				# FG Rudder/Aileron and Elevator data range -1.0 to 1.0
				foreach x [lrange $servoPWData end-2 end-1] {
					lappend fgOutData [ expr { ( ( ( $x - 1000 ) /500.0 ) - 1.0) } ]					
				}
				set lastX [lindex $servoPWData end]
				# FG Throttle data range 0.0 to 1.0
				lappend fgOutData [ expr { ( $lastX - 1000 ) /1000.0 } ]
			
				#convert to string and add separators
				set fgOutString [join $fgOutData ","]				
				
        			#send to flightgear
				puts $S(comm.fgOutChan) $fgOutString
				flush $S(comm.fgOutChan)
				
				# update control positions
				::sap::win_displayControls $fgOutData
								
				#puts "OUT - $fgOutString"	;#DEBUG print
				
			} 
        	}		
	}
}

# serialPort 
# configures serial port for AP communication
#
# arguments:
#  comPort - Name of the serial port to use
#
proc ::sap::serialPort {comPort} {
	 
	variable S
	
	# open a serial channel
	set S(comm.SerialChan) [open $comPort RDWR]	
 	
	# configure channel, input reads lines, output writes binary
	fconfigure $S(comm.SerialChan) -blocking 0 \
		-mode $S(comm.SerialBaudRate),n,8,1 \
	 	-buffering line \
	 	-buffersize 4096 \
	 	-handshake none \
	 	-translation {auto binary}
	
	 # declare event handler for incoming data 
	fileevent $S(comm.SerialChan) readable \
	 	[list ::sap::apSerialRead $S(comm.SerialChan)]	   
	
	# save handle for channel
	#set S(comm.SerialChan) $fh		 
} 
 


###################################################################################################################
#TCP FlightGear Input ROUTINES

#fgReadHandler - Event handler for incoming data from flightgear
# arguments:
#  fgChan - handle for TCP channel
#
proc ::sap::fgReadHandler fgChan {	
	 variable S
	
	 ;# read input data from flightgear
	 set FGDataIn [gets $fgChan] 	
	 
	 # configure udp connection for returning data if we haven't already
	 if {$S(comm.fgOutFlag) == 0} {		
	 	::sap::openOutChan
	 }
	 
	 # process data for ArduPilot
	 ::sap::imulate $FGDataIn			
	 	 
	 if {[eof $fgChan]} {    ;# client gone or finished - should not see this
     		close $fgChan        ;# release the servers client channel
  	}
} 
 
 
# create and configure a server to handle data from flightgear
#
proc ::sap::fgAccept {chan addr port} {	   
		
     	fconfigure $chan -buffering line    ;# automate flushing -
     	fileevent $chan readable \
	 	[list ::sap::fgReadHandler $chan]	  ;# set up event handler for incoming data	
}
 

 
###################################################################################################################
#UDP FlightGear Output ROUTINES 
 
 
# event handler for udp data input - stub, currently unused
#
proc ::sap::fgUdpDataReceived {channel} {
      set packet [read $channel]
      set peer [fconfigure $channel -peer]
      # Do something with the data and/or the peer address information
      return
}


# openOutChan
# connects to flightgear to deliver control info via udp
# opens, configures and flags the udp channel
# called when we first receive data from flightgear
# 
proc ::sap::openOutChan {} {
	variable S
	
	set chan [udp_open]
	fconfigure $chan -remote [list $S(comm.fgHost) $S(comm.fgServicePort)] \
	    	-buffering line \
	    	-blocking 0
	# declare an event handler for incoming data
	fileevent $chan readable [list ::sap::fgUdpDataReceived $chan]
    	
	set S(comm.fgOutFlag) 1	;# flag channel open and configured 
	
	set S(comm.fgOutChan) $chan		;# save channnel handle	
}




###################################################################################################################
# TCP GroundStation ROUTINES - currently unused 
# stubs for client or server connections 

# handleComm - Event handler for incoming data from ArduPilot
#
proc ::sap::gsHandleComm gsChan {
	
        variable S
        
        ;# read input data from flightgear
        set gsDataIn [gets $gsChan] 	
	 
	lappend ::sap::S(log.Data) $gsDataIn
	
	if {[string range $gsDataIn 0 2] == "!!!"} {
        	# process data for ArduPilot
        	::sap::displayInData $gsDataIn			
	} 	 
	if {[eof $gsChan]} {    ;# client gone or finished
     		close $gsChan        ;# release the servers client channel	}
}  
 

# gsAccept - create and configure a server to handle imulator input
#
proc ::sap::gsAccept {chan addr port} {	   ;# Make a proc to accept connections
	variable S
     	fconfigure $chan -buffering line    ;# automate flushing -
     	fileevent $chan readable \
	 	[list ::sap::gsHandleComm $chan]	  ;# set up event handler for incoming data
	 set S(comm.gsServerChan) $chan
}
	


###################################################################################################################
#Comms initialisation 

# commConnect - opens serial (AP) and TCP (FG output) channels
# handler for GUI 'Connect' button widget
# 
proc ::sap::commConnect {} {
	variable S
	
	# check user has specifed a serial port
	if {$S(comm.SerialPort) == ""} {
		set answer [tk_messageBox -title "No Serial Port Specified" -message "Please choose a serial port " -type ok ]
		return
	}
	
        # create a tcp server to handle data from flightgear
#	if { [catch { socket -server ::sap::fgAccept $S(comm.fgPort) } fid] } {
#		#error handler
#		set answer [tk_messageBox -title "Communication Error!" \
#		 	-message "Could not open tcp server for Simulator \n$fid" -type ok ]
#    		
#		return
#		}
        # Create a tcp server socket to handle data from flightgear
	set S(comm.fgChan) [socket -server ::sap::fgAccept $S(comm.fgPort)]		
	
	# configure and open serial comms to ArduPilot
	if { [catch {::sap::serialPort $S(comm.SerialPort)} fid] } {
		#error handler
		set answer [tk_messageBox -title "Communication Error!" \
		 	-message "Could not open serial port  \n$fid" -type ok ]
		
		# no serial - stop tcp to FlightGear for clean start next attempt
    		close $S(comm.fgChan)
		
		return		
	}
	
       	# update the GUI connection buttons status
	::sap::win_StatusConnected
}


# commDisconnect - closes serial (AP) and TCP (FG output) channels
# handler for GUI 'Disconnect' button widget
# 
proc ::sap::commDisconnect {} {
	variable S
	
	# close TCP channel to FlightGear
	close $S(comm.fgChan)
	
	# close serial channel
	close $S(comm.SerialChan)
	
	# update GUI 
	::sap::win_StatusDisconnected
}


